
#ifndef _CAMERA_CAL_CALIBRATE_HH_
#define _CAMERA_CAL_CALIBRATE_HH_

#include "PinholeCamera.h"
#include "Geometry.h"
#include "Ravl/Vector.hh"
#include "Ravl/Matrix.hh"
#include "Mytime.h"
namespace CameraCalN
{
	using namespace RavlN;


	class WandCalBodyC : public RCBodyC
	{
	public:
		WandCalBodyC(PinholeArrayC _camera, 
			UIntT _frames, 
			bool opt_focal,
			UIntT _dynamic_iterations,
			RealT _max_init_rms);
            MyTimer timecount;
	public:
		RealT NewObs(const SArray1dC<CalObservation2dC>& obs0,
			const SArray1dC<CalObservation2dC>& obs1);

		bool BatchUpdate(UIntT iterations);

		RealT RMSReprojectionError();

		void Scale(RealT s);

		RealT UpdateErrorRMSBest()
		{ return error_rms_best; }

		RealT UpdateErrorRMSCurrent()
		{ return error_rms_current; }

		RealT UpdateErrorPixelMax()
		{ return error_pixel_max; }

	public:
		UIntT NumPoints() 
		{ return npoints; }

		const CalObservation2dC& GetObs(UIntT icam, UIntT ipoint)
		{ return obs[icam][ipoint]; }

		UIntT TotalObsAdded() const
		{ return total_frames; }
        void outdatax(OStreamC& ff)
		{
			ff<<xb0<<endl;
		};
	protected:
		bool InitGeometry();
		bool LMUpdate();

	protected:
		UIntT ncameras;
		UIntT nframes;
		UIntT npoints;
		UIntT nparams_cam;
		UIntT nparams_point;
		UIntT nparams_point_constraint;
		UIntT nobs;
		bool opt_focal;
		RealT wandlength;
		PinholeArrayC camera;
		bool have_init_geometry;
		UIntT add_index;
		UIntT total_frames;
		SArray1dC< CalObservation2dArrayC > obs;
		SArray1dC< Vector2dC > focal0;
		RealT max_init_rms;
		UIntT dynamic_iterations;

		// current error values
		RealT error_rms_current;
		RealT error_rms_best;
		RealT error_pixel_max;

		// LM best guess state params
		VectorC xa0;
		VectorC xb0;
		RealT lambda;
	};

	class WandCalC : public RCHandleC<WandCalBodyC>
	{
	public:
		WandCalC()
		{
		}

		WandCalC(PinholeArrayC camera, 
			UIntT frames, 
			bool opt_focal,
			UIntT dynamic_iterations,
			RealT max_init_rms)
			: RCHandleC<WandCalBodyC> (*new WandCalBodyC(camera, 
			frames, 
			opt_focal,
			dynamic_iterations,
			max_init_rms))
		{
		}

	public:
         void outdataxx(OStreamC& ff)
		 {
		   Body().outdatax(ff);
		 }
		RealT NewObs(const SArray1dC<CalObservation2dC>& obs0,
			const SArray1dC<CalObservation2dC>& obs1)
		{ return Body().NewObs(obs0,obs1); }

		UIntT TotalObsAdded() const
		{ return Body().TotalObsAdded(); }

		bool BatchUpdate(UIntT iterations)
		{
			return Body().BatchUpdate(iterations);
		}

		RealT RMSReprojectionError()
		{
			return Body().RMSReprojectionError();
		}

		void Scale(RealT s)
		{
			Body().Scale(s);
		}

		RealT UpdateErrorRMSBest()
		{ return Body().UpdateErrorRMSBest(); }

		RealT UpdateErrorRMSCurrent()
		{ return Body().UpdateErrorRMSCurrent(); }

		RealT UpdateErrorPixelMax()
		{ return Body().UpdateErrorPixelMax(); }

	public:
		UIntT NumPoints() 
		{ return Body().NumPoints(); }

		const CalObservation2dC& GetObs(UIntT icam, UIntT ipoint)
		{ return Body().GetObs(icam, ipoint); }
	};
}

#endif
